"""
航线规划器

date: 2025/8/19
author: SiHeng Tang
file: mission_waypoints_gen.py
copyright(c) DFSA Eastern Software
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""

from helpers.geo_math_trans import GeoVector
from mj_errors import MJRuntimeError


def __plan_gicg_path(wp_arg: dict):
    """
    规划 GICG 任务模板
    :param wp_arg: 函数参数
    :return: 三维航点列表
    """
    wp_default_arg = {
        "home_position_lat": -35.362765,
        "home_position_lon": 149.165148,
        "target_area_lat": -35.365820,
        "target_area_lon": 149.163110,
        "target_area_relative_alt": 0.0,
        "target_area_circle_r": 35.35,
        "run_way_center_lat": -35.362765,
        "run_way_center_lon": 149.165148,
        "run_way_center_relative_alt": 0.0,
        "run_way_width": 20.0,
        "run_way_length": 50.0,
        "run_way_direction": 351.0,
        "run_way_entrance_distance": 300.0,
        "takeoff_direction": 351.0,
        "takeoff_length": 50.0,
        "takeoff_alt": 30.0,
        "takeoff_alt_threshold": 25,
        "trans_line_r": 200.0,
        "trans_wp_index": 3,
        "guided_circle_r": 20.0,
        "guided_update_rate_hz": 1,
        "approaching_r": 100.0,
        "approaching_alt": 20.0,
        "basic_alt": 50.0,
    }

    wp_default_arg.update(wp_arg)

    wp_gv_list = []

    home_gv = GeoVector(lat=float(wp_default_arg["home_position_lat"]),
                        lon=float(wp_default_arg["home_position_lon"]))
    tar_area_gv = GeoVector(lat=float(wp_default_arg["target_area_lat"]),
                            lon=float(wp_default_arg["target_area_lon"]))
    runway_gv = GeoVector(lat=float(wp_default_arg["run_way_center_lat"]),
                          lon=float(wp_default_arg["run_way_center_lon"]))

    runway_cl_gv_unit = GeoVector.get_gv_from_hdg(wp_default_arg["run_way_direction"])
    takeoff_gv_unit = GeoVector.get_gv_from_hdg(wp_default_arg["takeoff_direction"])

    # takeoff vector
    takeoff_gv = takeoff_gv_unit * wp_default_arg["takeoff_length"]
    wp_gv_list.append(home_gv.add_difference(takeoff_gv))

    # trans vector
    diff_gv_unit = runway_gv.diff(tar_area_gv).get_unit_vector()
    trans_gv = diff_gv_unit * wp_default_arg["trans_line_r"]
    wp_gv_list.append(home_gv.add_difference(trans_gv))

    # reenter vector
    diff_gv_unit = wp_gv_list[1].diff(tar_area_gv).get_unit_vector()
    reenter_gv = diff_gv_unit * 2 * wp_default_arg["guided_circle_r"]
    wp_gv_list.append(tar_area_gv.add_difference(reenter_gv))

    # way_back vector
    runway_cl_gv_rv_unit = runway_cl_gv_unit * -1.
    runway_en_point_gv = runway_gv.add_difference(
        runway_cl_gv_rv_unit * wp_default_arg["run_way_entrance_distance"])
    diff_gv_unit = tar_area_gv.diff(runway_en_point_gv).get_unit_vector()
    wp_gv_list.append(tar_area_gv.add_difference(diff_gv_unit * wp_default_arg["target_area_circle_r"] * 1.5))

    # runway entrance 前面算过了
    wp_gv_list.append(runway_en_point_gv)

    # approaching
    wp_gv_list.append(runway_gv.add_difference(runway_cl_gv_rv_unit * wp_default_arg["approaching_r"]))

    # land
    wp_gv_list.append(runway_gv)
    wp_list = []

    # 这里要处理高度数据比较灵活，没用列表推导式
    for i, wp in enumerate(wp_gv_list):
        # takeoff 高度
        if i == 0:
            alt = wp_default_arg["takeoff_alt"]

        # approaching 进近高度
        elif i == 5:
            alt = wp_default_arg["approaching_alt"]

        # 最后一个降落航点高度为 0
        elif i == 6:
            alt = 0

        # 默认巡航高度
        else:
            alt = wp_default_arg["basic_alt"]

        wp_list.append([wp.lat, wp.lon, alt])

    return wp_list


def __plan_gicg_v2_path(wp_arg: dict):
    """
    规划 GICG 任务模板
    :param wp_arg: 函数参数
    :return: 三维航点列表
    """
    wp_default_arg = {
        "takeoff_position_lat": 0,
        "takeoff_position_lon": 0,
        "takeoff_direction": 0,
        "takeoff_length": 70,
        "takeoff_alt": 12,
        "cruise_alt": 35,
        "trans_line_distance": 100,
        "target_position_lat": 0,
        "target_position_lon": 0,
        "reenter_line_distance": 200,
        "landing_runway_center_lat": 0,
        "landing_runway_center_lon": 0,
        "landing_runway_direction": 0,
        "way_back_arc_angle": 10,
        "way_back_distance": 400,
        "way_back_alt": 30,
        "runway_entrance_arc_angle": 5,
        "runway_entrance_distance": 200,
        "runway_entrance_alt": 20,
        "approaching_arc_angle": 0,
        "approaching_distance": 100,
        "approaching_alt": 9
    }
    wp_default_arg.update(wp_arg)
    waypoints_list = []

    takeoff_point_gv = GeoVector(lat=float(wp_default_arg["takeoff_position_lat"]),
                                 lon=float(wp_default_arg["takeoff_position_lon"]))
    target_area_gv = GeoVector(lat=float(wp_default_arg["target_position_lat"]),
                               lon=float(wp_default_arg["target_position_lon"]))
    landing_runway_gv = GeoVector(lat=float(wp_default_arg["landing_runway_center_lat"]),
                                  lon=float(wp_default_arg["landing_runway_center_lon"]))

    takeoff_direction_unit = GeoVector.get_gv_from_hdg(wp_default_arg["takeoff_direction"])

    # takeoff waypoint gv
    takeoff_gv = takeoff_direction_unit * wp_default_arg["takeoff_length"]
    takeoff_wp_gv = takeoff_point_gv.add_difference(takeoff_gv)
    waypoints_list.append([takeoff_wp_gv.lat, takeoff_wp_gv.lon, wp_default_arg["takeoff_alt"]])

    # trans waypoint gv
    diff_gv_unit = takeoff_point_gv.diff(target_area_gv).get_unit_vector()
    trans_gv = diff_gv_unit * wp_default_arg["trans_line_distance"]
    trans_wp_gv = takeoff_point_gv.add_difference(trans_gv)
    waypoints_list.append([trans_wp_gv.lat, trans_wp_gv.lon, wp_default_arg["cruise_alt"]])

    # reenter waypoint gv
    diff_gv_unit = trans_wp_gv.diff(target_area_gv).get_unit_vector()
    reenter_gv = diff_gv_unit * wp_default_arg["reenter_line_distance"]
    reenter_wp_gv = target_area_gv.add_difference(reenter_gv)
    waypoints_list.append([reenter_wp_gv.lat, reenter_wp_gv.lon, wp_default_arg["cruise_alt"]])

    # way_back runway_entrance approaching waypoint gv
    way_back_arc_dir_gv_unit = GeoVector.get_gv_from_hdg(float(wp_default_arg["landing_runway_direction"]) +
                                                         float(wp_default_arg["way_back_arc_angle"])) * -1.0
    way_back_gv = way_back_arc_dir_gv_unit * wp_default_arg["way_back_distance"]
    way_back_wp_gv = landing_runway_gv.add_difference(way_back_gv)
    waypoints_list.append([way_back_wp_gv.lat, way_back_wp_gv.lon, wp_default_arg["way_back_alt"]])

    runway_entrance_arc_dir_gv_unit = GeoVector.get_gv_from_hdg(float(wp_default_arg["landing_runway_direction"]) +
                                                                float(
                                                                    wp_default_arg["runway_entrance_arc_angle"])) * -1.0
    runway_entrance_gv = runway_entrance_arc_dir_gv_unit * wp_default_arg["runway_entrance_distance"]
    runway_entrance_wp_gv = landing_runway_gv.add_difference(runway_entrance_gv)
    waypoints_list.append([runway_entrance_wp_gv.lat, runway_entrance_wp_gv.lon, wp_default_arg["runway_entrance_alt"]])

    approaching_arc_dir_gv_unit = GeoVector.get_gv_from_hdg(float(wp_default_arg["landing_runway_direction"]) +
                                                            float(wp_default_arg["approaching_arc_angle"])) * -1.0
    approaching_gv = approaching_arc_dir_gv_unit * wp_default_arg["approaching_distance"]
    approaching_wp_gv = landing_runway_gv.add_difference(approaching_gv)
    waypoints_list.append([approaching_wp_gv.lat, approaching_wp_gv.lon, wp_default_arg["approaching_alt"]])

    # land
    waypoints_list.append([landing_runway_gv.lat, landing_runway_gv.lon, 0])

    return waypoints_list


ALL_PLANNER = {
    "gicg": __plan_gicg_path,
    "gicg_v2": __plan_gicg_v2_path
}


def plan_path(wp_arg: dict, planner: str, **kwargs):
    """
    选择一个航线规划器，使用相应的航向参数生成三维航线。
    :param wp_arg: 航线参数，字典，每一个生成函数都不相同
    :param planner: 规划器
    :param kwargs: 额外参数，保留
    :return: 三维航点，使用二维列表表示 [[lat, lon, alt_relative], ...]
    """
    if planner not in ALL_PLANNER:
        raise MJRuntimeError(MJRuntimeError.PROCESSOR_ERROR, f"call for an unavailable waypoint planner "
                                                             f"{planner}, check if any module lost?")

    return ALL_PLANNER[planner](wp_arg)
